/*
* @Author: obsidian
* @Date:   2016-03-01 11:08:57
* @Last Modified by:   obsidian
* @Last Modified time: 2016-05-22 17:58:48
*/
#include "math.h"
#include "driver/math/inc/math.h"
#include "driver/sd/file.h"
#include "car.h"
#include "module/GPS/GPS.h"
#include "module/steer/steer.h"
#include "module/gpsteer/gpsteer.h"
#include "module/stopsteer/stopsteer.h"
#include "module/brake/brake.h"
#include "module/camera/camera.h"
#include "module/ledboard/ledboard.h"

#include "control/GPSpath/GPSpath.h"

void strjoint(unsigned char *filename);
void car_init(void);
void car_save(void);

struct Car car = 
{
	0,//
    "config1/",
    strjoint,
    .set_parameter = car_init,    
    car_save,        
};

struct Car_Para car_para[] = 
{
	{//car 0  黄色帆小车
        .brake_midvalue0     = 1170,//上坡的时候不会蹭着赛道的极限
		.brake_midvalue1	 = 1170,//下坡的时候不会蹭着赛道的极限
        .brake_limit         = 1170,//轮子不会悬空的极限 
		.stop_value      	 = 1130 - 100,//撞杆之后刹车的值
		 
        .steer_midvalue      = 1550,//1220,
        .steer_limit         = {2130,850},
        .steer_ratio         = 30.0,
		 
        .gpsteer_midvalue0   = 925,//左边赛道初始位置
		.gpsteer_midvalue1	 = 600,//右边赛道初始位置
        .gpsteer_limit       = {2100,2100},//左边赛道收回-右边赛道收回
        .gpsteer_ratio       = 2.0,
		
		//撞杆机构
        .stopsteer_midvalue0 = 1250,//右边赛道偏左
		.stopsteer_midvalue1 = 1230,//左边赛道偏右
		.stopsteer_limit     = {2100,450},
		 
        .direction           = 0 ,  //left:2 right:0
		 
// 		.r_start_x			 =0,
// 		.r_start_y			 = -680 + 100,//边框的起点
// 		.r_start_angle		 = 0,//
// 		
// 		.l_start_x			 = 0,
// 		.l_start_y			 = -680 + 100,//边框的起点
// 		.l_start_angle		 = 0,//
		
		.r_start_x			 = 100,
		.r_start_y			 = 475,//边框的起点
		.r_start_angle		 = -15.3,//
		
		.l_start_x			 = -100,
		.l_start_y			 = 475,//边框的起点
		.l_start_angle		 = 15.3,//	
		
		.end_to_CenterOfRearwheel = 60,
        .car_len             = 280,
		
		.gyro_float			 = -488.2163,
        .gyro_convert1       = 1.4527665,
        .gyro_convert2       = 1.45323,
        .encoder1_eff        = -0.10294538,//-0.085180,
        .encoder2_eff        = 0.1579700,//-0.08353,;;;;;;;0.10293008
		 
        .led_dis             = 330,
        .ledboard_status     = 1,
		 
		.camera_center       = 173,
        .camera_status       = 0,
    },
	  {//car 1  ！！！！！注意   这是红色帆小车
		.brake_midvalue0     = 1400,//上坡的时候不会蹭着赛道的极限
		.brake_midvalue1	 = 1400,//下坡的时候不会蹭着赛道的极限
        .brake_limit         = 1140,//轮子不会悬空的极限 
		.stop_value      	 = 1140 - 100,//撞杆之后刹车的值
		 
		.steer_midvalue      = 1350,
		.steer_limit         = {1530,1170},
		.steer_ratio         = -3.8,
		 
        .gpsteer_midvalue0   = 700,//左边赛道初始位置
		.gpsteer_midvalue1	 = 600,//右边赛道初始位置
        .gpsteer_limit       = {2100,2100},//左边赛道收回-右边赛道收回
        .gpsteer_ratio       = 2.0,
		
		//撞杆机构
		.stopsteer_midvalue0 = 1480,//右半场偏左
		.stopsteer_midvalue1 = 1400,//左半场偏右
		.stopsteer_limit     = {2180,560},
		 
		.direction           = 2,  //left:2 right:0
		 
		.r_start_x			 = 100,
		.r_start_y			 = 480,//边框的起点
		.r_start_angle		 = -15.3,//
		
		.l_start_x			 = -100,
		.l_start_y			 = 480,//边框的起点
		.l_start_angle		 = 15.3,//	
		
// 		.r_start_x			 = 0,
// 		.r_start_y			 = -680,//边框的起点
// 		.r_start_angle		 = 0,//
// 		
// 		.l_start_x			 = 0,
// 		.l_start_y			 = -680,//边框的起点
// 		.l_start_angle		 = 0,//

		.car_len             = 285.5,
		.end_to_CenterOfRearwheel = 60,
		
		.gyro_float			 = 99.14571,
		.gyro_convert1       = 3600/2277.094,
		.gyro_convert2       = 3600/2275.794,
		
		.encoder1_eff		 = -2985.0 / 35152.0,
		.encoder2_eff		 = 2985.0 / 34929.0,
// 		.encoder1_eff        =  9000.0/(34847.94+34773.79+34781.93),//
// 		.encoder2_eff        = -9000.0/(34424.06+34573.85+34599.94),//
		 
		.led_dis             = 330,
		.ledboard_status     = 0,
		 
		.camera_center       = 330,
		.camera_status       = 0,
		.offset_y            = 0,
	},
};


/**
 * append the file name to the end of directionary car.path
 * @param filename
 */
void strjoint(unsigned char *filename)
{
    unsigned char i = 0;
    zero((char *)&car.path[8], 12);
    car.path[6] = car.version + '0';
    for(i = 8;i < 20 && *filename != 0; i ++, filename ++)
    {
        car.path[i] = *filename;
    }
}

static struct Car dup_car;

void car_init()
{
        unsigned int k = 0;
        f_open(&fil,"car",FA_READ);
        if(fil.fs != 0)
        {
            f_read(&fil, (char *)&dup_car, sizeof(struct Car), &k);
            car.version = dup_car.version;
            file_close();
        }
        brake.midvalue0			    = car_para[car.version].brake_midvalue0;
		brake.midvalue1			    = car_para[car.version].brake_midvalue1;
        brake.limit 			    = car_para[car.version].brake_limit;
		brake.stop_value 		    = car_para[car.version].stop_value;
		    
		steer.midvalue 			    = car_para[car.version].steer_midvalue;
        steer.limit[0] 			    = car_para[car.version].steer_limit[0];
        steer.limit[1] 			    = car_para[car.version].steer_limit[1];
        steer.ratio 			    = car_para[car.version].steer_ratio;
		    
        gpsteer.midvalue0 		    = car_para[car.version].gpsteer_midvalue0;
		gpsteer.midvalue1 		    = car_para[car.version].gpsteer_midvalue1;
        gpsteer.limit[0] 		    = car_para[car.version].gpsteer_limit[0];
        gpsteer.limit[1] 		    = car_para[car.version].gpsteer_limit[1];
		gpsteer.ratio   	 	    = car_para[car.version].gpsteer_ratio;
    
        //撞杆机构    
        stopsteer.midvalue0         = car_para[car.version].stopsteer_midvalue0;
		stopsteer.midvalue1         = car_para[car.version].stopsteer_midvalue1;
        stopsteer.limit[0]          = car_para[car.version].stopsteer_limit[0];
        stopsteer.limit[1]          = car_para[car.version].stopsteer_limit[1];
		    
        gps.direction 			    = car_para[car.version].direction;
		
		if(gps.direction == 0)
		{
			gps.start_angle				= car_para[car.version].r_start_angle;
		
			gps.start_point.x		    = car_para[car.version].r_start_x + car_para[car.version].end_to_CenterOfRearwheel*cos(PI/2-car_para[car.version].r_start_angle*3.1415926/180.0);
			gps.start_point.y		    = car_para[car.version].r_start_y + car_para[car.version].end_to_CenterOfRearwheel*sin(PI/2-car_para[car.version].r_start_angle*3.1415926/180.0);
		}    
		else if(gps.direction == 2)
		{
			gps.start_angle				= car_para[car.version].l_start_angle;
		
			gps.start_point.x		    = car_para[car.version].l_start_x + car_para[car.version].end_to_CenterOfRearwheel*cos(PI/2-car_para[car.version].l_start_angle*3.1415926/180.0);
			gps.start_point.y		    = car_para[car.version].l_start_y + car_para[car.version].end_to_CenterOfRearwheel*sin(PI/2-car_para[car.version].l_start_angle*3.1415926/180.0);
		}    
        gps.car_len 			    = car_para[car.version].car_len;

		gyro.gyro_calibration.gyro_float = car_para[car.version].gyro_float;
        gyro.gyro_convert1 		    = car_para[car.version].gyro_convert1;
        gyro.gyro_convert2 		    = car_para[car.version].gyro_convert2;
        encoder_property[0].convert = car_para[car.version].encoder1_eff;
        encoder_property[1].convert = car_para[car.version].encoder2_eff;
        
        gps.led_dis 				= car_para[car.version].led_dis;
        ledboard.status       		= car_para[car.version].ledboard_status;
		
		gpspath.camera_center 		= car_para[car.version].camera_center;
        camera.status         		= car_para[car.version].camera_status;
		gpspath.offset_y            = car_para[car.version].offset_y;


}

void car_save()
{
        unsigned int k = 0;
        f_open(&fil,"car",FA_WRITE | FA_CREATE_ALWAYS);
        if(fil.fs == 0){return;}
        f_write(&fil, (char *)&car, sizeof(struct Car), &k);
        file_close();
}
